package edu.wpi.first.wpilibj.frc;

/*
 * ----------------------------------------------------------------------------
 */
/*
 * Copyright (c) FIRST 2008. All Rights Reserved.
 */
/*
 * Open Source Software - may be modified and shared by FRC teams. The code
 */
/*
 * must be accompanied by the FIRST BSD license file in the root directory of
 */
/*
 * the project.
 */
/*
 * ----------------------------------------------------------------------------
 */
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.image.ColorImage;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class OldRobot extends IterativeRobot {

    RobotDrive robotDrive;
    Joystick gamePad;
    Joystick armJoystick;
    Jaguar leftArm;
    Jaguar rightArm;
    Compressor compressor;
    Solenoid hand;
    Solenoid extension1;
    Solenoid minislide;
    DigitalInput armForwardLimit;
    DigitalInput armRearLimit;
    DigitalInput leftLineTracker;
    DigitalInput centerLineTracker;
    DigitalInput rightLineTracker;
    Servo minirelease;
    AnalogChannel armpot1;
    Encoder encoderleft;
    Encoder encoderright;
    Ultrasonic ultrasonic1;
    double ultrasonicDistance;
    Dashboard dashboard;
    int armPosition;
    int positions[] = {0, 843, 786, 690, 843, 767, 690};
    //The first 3 values starting with 843 are for the lower pegs and the 3 ones
    // after taht are for the higher pegs
    AxisCamera camera;
    Timer autoTimer;
    Timer revTimer;
    Timer shootwait;
    boolean done = false;
    boolean mini = false;
    boolean ultrasonicAtEnd = false;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {

        robotDrive = new RobotDrive(1, 2, 3, 4);

        gamePad = new Joystick(1);
        armJoystick = new Joystick(2);

        leftArm = new Jaguar(5);
        rightArm = new Jaguar(6);

        compressor = new Compressor(1, 1);

        hand = new Solenoid(1);
        extension1 = new Solenoid(2);
        minislide = new Solenoid(4);

        armForwardLimit = new DigitalInput(2);
        armRearLimit = new DigitalInput(3);

        armpot1 = new AnalogChannel(1);

        minirelease = new Servo(8);

        compressor.start();

        armPosition = 0;

        camera = AxisCamera.getInstance();

        //leftLineTracker = new DigitalInput (4);
        // centerLineTracker = new DigitalInput (5);
        // rightLineTracker = new DigitalInput (6);

        encoderleft = new Encoder(4, 5);
        encoderright = new Encoder(6, 7);
        double inchesPerPulse = 1.0;
        encoderleft.setDistancePerPulse(inchesPerPulse);
        encoderright.setDistancePerPulse(inchesPerPulse);

        ultrasonic1 = new Ultrasonic(8, 9);

        autoTimer = null;
        revTimer = null;
        shootwait = null;
        done = false;
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Watchdog.getInstance().feed();

        // read digital switches from the driver station for config settings
        boolean doNothing = DriverStation.getInstance().getDigitalIn(1);
        boolean useEncoders = DriverStation.getInstance().getDigitalIn(3);
        boolean useUltrasonic = DriverStation.getInstance().getDigitalIn(4);

        // read analog sliders from the driver station for motor settings
        double fwdSpeed = DriverStation.getInstance().getAnalogIn(1);
        double revSpeed = -1 * DriverStation.getInstance().getAnalogIn(2);
        double slwSpeed = DriverStation.getInstance().getAnalogIn(3);
        double stpSpeed = 0.0;
        double rev1Speed = -.75;
        double motorSpeed = stpSpeed;

//        System.out.println("Throttles are: FWD=" + fwdSpeed + ", REV=" + revSpeed + ", SLOW=" + slwSpeed);

        if (doNothing == false) {
/////////////////////// FIX THE NEXT 2 LINES BEFORE RUNNING /////////////////////
            boolean move = getAutonomousArmPosition();
//            boolean move = true;
            boolean fire = false;

            if (move == true) {
                if ((useEncoders != true) && (useUltrasonic != true)) {
                    double stopTime = 1.65;
                    double revTime = 0.5;

                    if (autoTimer == null) {
                        autoTimer = new Timer();
                        autoTimer.reset();
                        autoTimer.start();
                    }

                    if (autoTimer.get() <= stopTime) {
                        System.out.println("Stop time is " + stopTime + " Timer is " + autoTimer.get());
                        motorSpeed = fwdSpeed;
                    } else {
                        if (revTimer == null) {
                            revTimer = new Timer();
                            revTimer.reset();
                            revTimer.start();
                        }

                        if (revTimer.get() <= revTime) {
                            motorSpeed = revSpeed;
                        } else {
                            motorSpeed = stpSpeed;
                            fire = true;
                        }
                    }
                } else if (useUltrasonic == true) {
                    double slowDistance = 74.0; // 74.0 43.0 oppps 20 and 26 don't work...
                    double endDistance = 43.0;
                    if (ultrasonic1.isRangeValid() == true) {
                        System.out.println("Ultrasonic distance = " + ultrasonicDistance);
                        ultrasonicDistance = ultrasonic1.getRangeInches();
                    } else {
                        System.out.println("Range is not valid");
                    }

                    if (done == false) {
                        if (ultrasonicDistance > slowDistance) {
                            motorSpeed = fwdSpeed;
                        } else if (ultrasonicDistance > endDistance) {
                            motorSpeed = slwSpeed;
                        } else {
                            motorSpeed = stpSpeed;
                            if (shootwait == null) {

                                shootwait = new Timer();
                                shootwait.reset();
                                shootwait.start();
                            }
                            if (shootwait.get() >= .5) {
                                fire = true;
                            }

                            if (shootwait.get() >= 1.5) {
                                fire = true;
                                extension1.set(false);
                                motorSpeed = rev1Speed;
                            }

                            if (shootwait.get() >= 2) {
                                fire = true;
                                extension1.set(false);
                                motorSpeed = stpSpeed;
                                done = true;
                            }
                        }
                    } else {
                        extension1.set(false);
                        motorSpeed = stpSpeed;
                    }
                    // Code Summary - Bot will approach wall, when it stops, it
                    // will pause for a half second, then after the bot has been
                    // stopped for 1 second, it will reverse for 2 seconds away
                    // the pole. 

                } else if (useEncoders == true) {
                    // how far we need to move in inches
                    double stopAt = 203.0;
                    double slowAt = 155.0;

                    // get the left distance
                    double lDistance = encoderleft.getDistance();

                    // get the right distance
                    double rDistance = encoderright.getDistance();

                    //compute the average
                    double aDistance = (lDistance + rDistance) / 2;

                    System.out.println("Encoder Distances L=" + lDistance + ", R=" + rDistance + ", Avg=" + aDistance);

                    int lCount = encoderleft.get();
                    int rCount = encoderright.get();
//                    int aCount = (lCount + rCount)/2;
                    System.out.println("Encoder Counts L=" + lCount + ", R=" + rCount);

                    if (aDistance < slowAt) {
                        motorSpeed = fwdSpeed;
                    } else if (aDistance < stopAt) {
                        motorSpeed = slwSpeed;
                    } else {
                        fire = true;
                        motorSpeed = stpSpeed;
                    }
                }

                if (fire == true) {
                    hand.set(true);
                }

            }

            System.out.println("Motor throttle is set to " + motorSpeed);
            robotDrive.tankDrive(motorSpeed, motorSpeed);
        }
    }

    public void autonomousInit() {
        autoTimer = null;
        revTimer = null;
        shootwait = null;

        encoderleft.reset();
        encoderright.reset();
        encoderleft.start();
        encoderright.start();

        ultrasonic1.setEnabled(true);
        ultrasonic1.setAutomaticMode(true);
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Watchdog.getInstance().feed();
        // Drive Motors

        double leftDrive = gamePad.getRawAxis(2);
        double rightDrive = gamePad.getRawAxis(4);

        if ((gamePad.getRawButton(5) == true) || (gamePad.getRawButton(6) == true)
                || (gamePad.getRawButton(7) == true) || (gamePad.getRawButton(8) == true)) {
            leftDrive *= .5;
            rightDrive *= .5;
        }

        robotDrive.tankDrive(leftDrive, rightDrive);

        //System.out.println("leftDrive =" + leftDrive + ", rightDrive =" + rightDrive);

        // 87's Arm Drive goes soi soi soi soi soi soi soi
        getDesiredArmPosition();
        moveArmToPosition();

        boolean runGripper = armJoystick.getRawButton(1);
        hand.set(runGripper);

        // MY ROLF COPTER SOI SOI SOI SOI SOI SOI

        boolean ext1 = armJoystick.getRawButton(2);
        boolean limitForward = armForwardLimit.get();
        boolean limitRear = armRearLimit.get();

//       System.out.println("LimitFwd=" + limitForward + ", LimitRev=" + limitRear);
        if ((limitForward == true)) //|| (limitRear == true))
        {
            // ext1 = false;
        }
        extension1.set(ext1);

        if ((armJoystick.getRawButton(4)) && (!mini)) {
            minislide.set(true);
            mini = true;
        }

        if ((armJoystick.getRawButton(6) == true) && (mini == true)) {
            minirelease.set(.5);
        }

        //End of Development on 2/7/11 at 5:43PM/17:43

        updateDashboard();

        //System.out.println( "Throttle=" + armJoystick.getThrottle() );
    }

    double moveArm(int currentPosition) {
        int difference = armPosition - currentPosition;
        int delta = 10;
        double motor = 0;

        double armValue = (armJoystick.getRawAxis(2) * -1.0);
        double armDelta = 0.5;
        if ((armValue > armDelta) || (armValue < (armDelta * -1.0))) {
            armPosition = 0;
//        motor =  armJoystick.getRawAxis(2) * -1;

            if (armValue > armDelta) {
                motor = 1;
            } else {
                motor = -1;
            }
        }

        if (armPosition != 0) {
            if (difference > delta) {
                motor = 1;
            } else if (difference < (delta * -1)) {
                motor = -1;
            } else {
                motor = 0;
                armPosition = 0;
            }
        }

        /**
         * if( (armpot1.getValue() > 0) && (armpot1.getValue() < 500) ) {
         * extension1.set( false ); }
         *
         * if( (armpot1.getValue() < 500)) { extension1.set( false ); }
         *
         * if( (armpot1.getValue() > 1000)) { extension1.set( false ); }
         */
        return motor;
    }

    boolean getAutonomousArmPosition() {
        boolean topRow = DriverStation.getInstance().getDigitalIn(2);
        boolean move = false;

        if (armpot1.getValue() < positions[6]) {
            leftArm.set(1.0);
            rightArm.set(1.0);
        } else {
            leftArm.set(0.0);
            rightArm.set(0.0);
            move = true;

            if (topRow == true) {
                extension1.set(true);
            } else {
                extension1.set(false);
            }
        }

        if ((armJoystick.getRawButton(4) == true) && (mini == false)) {
            minislide.set(true);
            mini = true;
        }

        if ((armJoystick.getRawButton(6) == true) && (mini == true)) {
            minirelease.set(.5);
        }

        return move;
    }

    void getDesiredArmPosition() {

        if (armJoystick.getRawButton(7) == true) {
            armPosition = positions[6] + positions[0];
        } else if (armJoystick.getRawButton(9) == true) {
            armPosition = positions[5] + positions[0];
        } else if (armJoystick.getRawButton(11) == true) {
            armPosition = positions[4] + positions[0];
        } else if (armJoystick.getRawButton(8) == true) {
            armPosition = positions[3] + positions[0];
        } else if (armJoystick.getRawButton(10) == true) {
            armPosition = positions[2] + positions[0];
        } else if (armJoystick.getRawButton(12) == true) {
            armPosition = positions[1] + positions[0];
        }

    }

    void moveArmToPosition() {
        double armSetting = moveArm(armpot1.getValue());

        System.out.println("Pot=" + armpot1.getValue() + ", Arm setting=" + armPosition + ", motor=" + armSetting);
        boolean limitForward = armForwardLimit.get();
        boolean limitRear = armRearLimit.get();

        if (armSetting > 0) {

            if (limitForward == true) {
                armSetting = 0;
                positions[0] = armpot1.getValue();
                armPosition = 0;
            }
        } else if (armSetting < 0) {
            if (limitRear == true) {
                armSetting = 0;
            }
            // }
            // else if (armSetting < 0)
            //{
            // if(armpot1.getValue() >= 930)
            // {
            //armSetting = 0;
            // }
        }

        leftArm.set(armSetting);
        rightArm.set(armSetting);
    }

    /*
     * Developed on a super cool Dell Studio 1558 (with an i5-520m
     * (2.40ghz-2.93ghz), 4 gigs of ram, an ATi Radeon 4570 HD and more :)) that
     * is better than any macbook :P,no tom it is not by the the game! and made
     * with Windows 7 :D
     */
//windows XP is the best os software ever :P
    void updateDashboard() {
        Dashboard lowDashData = DriverStation.getInstance().getDashboardPackerLow();

        try {
            ColorImage image = camera.getImage();
            DriverStationLCD.getInstance().updateLCD();
            image.free();
        } catch (Exception e) {
            //do nothing, don't care
            //WELL I DO. YOU IDIOT! YOU HAVE TO CARE! --Dylan.
        }

        lowDashData.addCluster();
        {
            lowDashData.addCluster();
            {     //analog modules
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(1).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
                lowDashData.addCluster();

                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(2).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
            }
            lowDashData.finalizeCluster();

            lowDashData.addCluster();
            { //digital modules
                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 4;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 6;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayReverse());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

            }
            lowDashData.finalizeCluster();

            lowDashData.addByte(Solenoid.getAllFromDefaultModule());
        }
        lowDashData.finalizeCluster();
        lowDashData.commit();

    }
}
//Our Robot Code goes soi soi soi soi soi soi soi soi soi soi soi soi soi
//You lack of classes, however, does not. --Dylan.